
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_attitude_control.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stddef.h>

#include "mc_attitude_control.h"
/*-----------------------------------macro------------------------------------*/
// default gains for Copter and Sub
#define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT  0.15f   // Medium
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       初始化姿态控制器
  * @param[in]   att_ctrl  
  * @param[in]   dt  
  * @param[out]  
  * @retval      
  * @note        
  */
void attctrl_init(Attitude_ctrl * att_ctrl, Motors_HandleTypeDef* motors, float dt)
{
    p_ctrl_init(&att_ctrl->_p_angle_roll,AC_ATTITUDE_CONTROL_ANGLE_P);
    p_ctrl_init(&att_ctrl->_p_angle_pitch,AC_ATTITUDE_CONTROL_ANGLE_P);
    p_ctrl_init(&att_ctrl->_p_angle_yaw,AC_ATTITUDE_CONTROL_ANGLE_P);

    att_ctrl->_dt = dt;
    att_ctrl->_angle_boost = true;
    att_ctrl->_use_sqrt_controller = true;
    att_ctrl->_throttle_rpy_mix_desired = AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT;
    att_ctrl->_throttle_rpy_mix = AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT;
    att_ctrl->_throttle_in = 0.0f;
    att_ctrl->_althold_lean_angle_max = 0.0f;
    att_ctrl->_feedforward_scalar = 1.0f;

    att_ctrl->_motors = motors;

    quat_init(&(att_ctrl->_attitude_target));
    quat_init(&(att_ctrl->_attitude_ang_error));
    quat_init(&(att_ctrl->_att_data.attitude_body));
}

// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(Attitude_ctrl * att_ctrl, float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
    // Convert from centidegrees on public interface to radians
    float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
    float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
    float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);

    // calculate the attitude target euler angles
    quat_to_euler(&att_ctrl->_attitude_target,(Euler_t*)&att_ctrl->_euler_angle_target);

    // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
    if (att_ctrl->get_roll_trim_rad != NULL) {
        euler_roll_angle += att_ctrl->get_roll_trim_rad();
    }


    if (att_ctrl->_rate_bf_ff_enabled) {
        Vector3f_t euler_accel_max = {attctrl_get_accel_roll_max_radss(att_ctrl), attctrl_get_accel_pitch_max_radss(att_ctrl), attctrl_get_accel_yaw_max_radss(att_ctrl)};
        // translate the roll pitch and yaw acceleration limits to the euler axis
        const Vector3f_t euler_accel = attctrl_euler_accel_limit(&att_ctrl->_euler_angle_target, &euler_accel_max);

        // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
        // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
        // and an exponential decay specified by smoothing_gain at the end.
        att_ctrl->_euler_rate_target.x = attctrl_input_shaping_angle(math_wrap_PI(euler_roll_angle - att_ctrl->_euler_angle_target.x), att_ctrl->_input_tc, euler_accel.x, att_ctrl->_euler_rate_target.x, att_ctrl->_dt);
        att_ctrl->_euler_rate_target.y = attctrl_input_shaping_angle(math_wrap_PI(euler_pitch_angle - att_ctrl->_euler_angle_target.y), att_ctrl->_input_tc, euler_accel.y, att_ctrl->_euler_rate_target.y, att_ctrl->_dt);

        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        att_ctrl->_euler_rate_target.z = attctrl_input_shaping_ang_vel(att_ctrl->_euler_rate_target.z, euler_yaw_rate, euler_accel.z, att_ctrl->_dt);

        // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
        attctrl_euler_rate_to_ang_vel(&att_ctrl->_euler_angle_target, &att_ctrl->_euler_rate_target, &att_ctrl->_ang_vel_target);
        // Limit the angular velocity
        attctrl_ang_vel_limit(&att_ctrl->_ang_vel_target, radians(att_ctrl->_ang_vel_roll_max), radians(att_ctrl->_ang_vel_pitch_max), radians(att_ctrl->_ang_vel_yaw_max));
        // Convert body-frame angular velocity into euler angle derivative of desired attitude
        attctrl_ang_vel_to_euler_rate(&att_ctrl->_euler_angle_target, &att_ctrl->_ang_vel_target, &att_ctrl->_euler_rate_target);
    } else {
        // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
        att_ctrl->_euler_angle_target.x = euler_roll_angle;
        att_ctrl->_euler_angle_target.y = euler_pitch_angle;
        att_ctrl->_euler_angle_target.z += euler_yaw_rate * att_ctrl->_dt;
        // Compute quaternion target attitude
        quat_from_euler(&att_ctrl->_attitude_target, att_ctrl->_euler_angle_target.x, att_ctrl->_euler_angle_target.y, att_ctrl->_euler_angle_target.z);

        // Set rate feedforward requests to zero
        vec3_zero(&att_ctrl->_euler_rate_target);
        vec3_zero(&att_ctrl->_ang_vel_target);
    }

    // Call quaternion attitude controller
    attctrl_attitude_controller_run_quat(att_ctrl);

}

// Command a thrust vector and heading rate
void attctrl_input_thrust_vector_rate_heading(Attitude_ctrl * att_ctrl, const Vector3f_t* thrust_vector, float heading_rate_cds)
{
    // Convert from centidegrees on public interface to radians
    const float heading_rate = radians(heading_rate_cds * 0.01f);

    // calculate the attitude target euler angles
    quat_to_euler(&att_ctrl->_attitude_target,(Euler_t*)&att_ctrl->_euler_angle_target);

    // convert thrust vector and heading to a quaternion attitude
    const Quat_t thrust_vec_quat = attctrl_attitude_from_thrust_vector(*thrust_vector, 0.0f);

    // calculate the angle error in x and y.
    Vector3f_t attitude_error;
    float thrust_vector_diff_angle;
    Quat_t thrust_vec_correction_quat;
    float returned_thrust_vector_angle;
    attctrl_thrust_vector_rotation_angles(&thrust_vec_quat, &att_ctrl->_attitude_target, &thrust_vec_correction_quat, &attitude_error, &returned_thrust_vector_angle, &thrust_vector_diff_angle);
    
    if (att_ctrl->_rate_bf_ff_enabled) {
        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        att_ctrl->_ang_vel_target.x = attctrl_input_shaping_angle(attitude_error.x, att_ctrl->_input_tc, attctrl_get_accel_roll_max_radss(att_ctrl), att_ctrl->_ang_vel_target.x, att_ctrl->_dt);
        att_ctrl->_ang_vel_target.y = attctrl_input_shaping_angle(attitude_error.y, att_ctrl->_input_tc, attctrl_get_accel_pitch_max_radss(att_ctrl), att_ctrl->_ang_vel_target.y, att_ctrl->_dt);

        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        att_ctrl->_ang_vel_target.z = attctrl_input_shaping_ang_vel(att_ctrl->_ang_vel_target.z, heading_rate, attctrl_get_accel_yaw_max_radss(att_ctrl), att_ctrl->_dt);

        // Limit the angular velocity
        attctrl_ang_vel_limit(&att_ctrl->_ang_vel_target, radians(att_ctrl->_ang_vel_roll_max), radians(att_ctrl->_ang_vel_pitch_max), MIN(radians(att_ctrl->_ang_vel_yaw_max), attctrl_get_slew_yaw_rads(att_ctrl)));
    } else {
        Quat_t yaw_quat;
        Vector3f_t yaw_quat_axis_angle = {0.0f, 0.0f, heading_rate * att_ctrl->_dt};
        quat_from_axis_angle1(&yaw_quat, &yaw_quat_axis_angle);

        // set persisted quaternion target attitude
        att_ctrl->_attitude_target = quat_mult2(&att_ctrl->_attitude_target, &thrust_vec_correction_quat, &yaw_quat);

        // Set rate feedforward requests to zero
        vec3_zero(&att_ctrl->_euler_rate_target);
        vec3_zero(&att_ctrl->_ang_vel_target);
    }

    // Convert body-frame angular velocity into euler angle derivative of desired attitude
    attctrl_ang_vel_to_euler_rate(&att_ctrl->_euler_angle_target, &att_ctrl->_ang_vel_target, &att_ctrl->_euler_rate_target);

    // Call quaternion attitude controller
    attctrl_attitude_controller_run_quat(att_ctrl);
}

// Command a thrust vector, heading and heading rate
void attctrl_input_thrust_vector_heading(Attitude_ctrl * att_ctrl, const Vector3f_t* thrust_vector, float heading_angle_cd, float heading_rate_cds)
{
    // Convert from centidegrees on public interface to radians
    float heading_rate = math_constrain_float(radians(heading_rate_cds * 0.01f), -attctrl_get_slew_yaw_rads(att_ctrl), attctrl_get_slew_yaw_rads(att_ctrl));
    float heading_angle = radians(heading_angle_cd * 0.01f);

    // calculate the attitude target euler angles
    quat_to_euler(&att_ctrl->_attitude_target,(Euler_t*)&att_ctrl->_euler_angle_target);

    // convert thrust vector and heading to a quaternion attitude
    const Quat_t desired_attitude_quat = attctrl_attitude_from_thrust_vector(*thrust_vector, heading_angle);

    if (att_ctrl->_rate_bf_ff_enabled) {
        // calculate the angle error in x and y.
        Vector3f_t attitude_error;
        float thrust_vector_diff_angle;
        Quat_t thrust_vec_correction_quat;
        float returned_thrust_vector_angle;
        attctrl_thrust_vector_rotation_angles(&desired_attitude_quat, &att_ctrl->_attitude_target, &thrust_vec_correction_quat, &attitude_error, &returned_thrust_vector_angle, &thrust_vector_diff_angle);

        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        att_ctrl->_ang_vel_target.x = attctrl_input_shaping_angle(attitude_error.x, att_ctrl->_input_tc, attctrl_get_accel_roll_max_radss(att_ctrl), att_ctrl->_ang_vel_target.x, att_ctrl->_dt);
        att_ctrl->_ang_vel_target.y = attctrl_input_shaping_angle(attitude_error.y, att_ctrl->_input_tc, attctrl_get_accel_pitch_max_radss(att_ctrl), att_ctrl->_ang_vel_target.y, att_ctrl->_dt);
        att_ctrl->_ang_vel_target.z = attctrl_input_shaping_angle2(attitude_error.z, att_ctrl->_input_tc, attctrl_get_accel_yaw_max_radss(att_ctrl), att_ctrl->_ang_vel_target.z, heading_rate, attctrl_get_slew_yaw_rads(att_ctrl), att_ctrl->_dt);

        // Limit the angular velocity
        attctrl_ang_vel_limit(&att_ctrl->_ang_vel_target, radians(att_ctrl->_ang_vel_roll_max), radians(att_ctrl->_ang_vel_pitch_max), MIN(radians(att_ctrl->_ang_vel_yaw_max), attctrl_get_slew_yaw_rads(att_ctrl)));
    } else {
        // set persisted quaternion target attitude
        att_ctrl->_attitude_target = desired_attitude_quat;

        // Set rate feedforward requests to zero
        vec3_zero(&att_ctrl->_euler_rate_target);
        vec3_zero(&att_ctrl->_ang_vel_target);
    }

    // Convert body-frame angular velocity into euler angle derivative of desired attitude
    attctrl_ang_vel_to_euler_rate(&att_ctrl->_euler_angle_target, &att_ctrl->_ang_vel_target, &att_ctrl->_euler_rate_target);

    // Call quaternion attitude controller
    attctrl_attitude_controller_run_quat(att_ctrl);
}

Quat_t attctrl_attitude_from_thrust_vector(Vector3f_t thrust_vector, float heading_angle)
{
    const Vector3f_t thrust_vector_up = {0.0f, 0.0f, -1.0f};

    if (math_flt_zero(vec3_length_squared(&thrust_vector))) {
        thrust_vector = thrust_vector_up;
    } else {
        vec3_norm(&thrust_vector, &thrust_vector);
    }

    // the cross product of the desired and target thrust vector defines the rotation vector
    Vector3f_t thrust_vec_cross;
    vec3_cross(&thrust_vec_cross, &thrust_vector_up, &thrust_vector);

    // the dot product is used to calculate the angle between the target and desired thrust vectors
    const float thrust_vector_angle = acosf(math_constrain_float(vec3_dot(&thrust_vector_up, &thrust_vector), -1.0f, 1.0f));

    // Normalize the thrust rotation vector
    const float thrust_vector_length = vec3_length(&thrust_vec_cross);
    if (math_flt_zero(thrust_vector_length) || math_flt_zero(thrust_vector_angle)) {
        thrust_vec_cross = thrust_vector_up;
    } else {
        vec3_mult(&thrust_vec_cross, &thrust_vec_cross, 1.0f/thrust_vector_length);
    }

    Quat_t thrust_vec_quat;
    quat_from_axis_angle2(&thrust_vec_quat, &thrust_vec_cross, thrust_vector_angle);

    Quat_t yaw_quat;
    const Vector3f_t thrust_vector_down = {0.0f, 0.0f, 1.0f};
    quat_from_axis_angle2(&yaw_quat, &thrust_vector_down, heading_angle);

    return quat_mult(&thrust_vec_quat, &yaw_quat);
}

// Calculates the body frame angular velocities to follow the target attitude
void attctrl_attitude_controller_run_quat(Attitude_ctrl * att_ctrl)
{
    // This vector represents the angular error to rotate the thrust vector using x and y and heading using z
    Vector3f_t attitude_error;
    attctrl_thrust_heading_rotation_angles(att_ctrl, &att_ctrl->_attitude_target, &att_ctrl->_att_data.attitude_body, &attitude_error, &att_ctrl->_thrust_angle, &att_ctrl->_thrust_error_angle);

    // Compute the angular velocity corrections in the body frame from the attitude error
    att_ctrl->_ang_vel_body = attctrl_update_ang_vel_target_from_att_error(att_ctrl, &attitude_error);

    // ensure angular velocity does not go over configured limits
    attctrl_ang_vel_limit(&att_ctrl->_ang_vel_body, radians(att_ctrl->_ang_vel_roll_max), radians(att_ctrl->_ang_vel_pitch_max), radians(att_ctrl->_ang_vel_yaw_max));

    // rotation from the target frame to the body frame
    Quat_t attitude_body_inv;
    quat_inverse(&attitude_body_inv, &att_ctrl->_att_data.attitude_body);
    Quat_t rotation_target_to_body = quat_mult(&attitude_body_inv, &att_ctrl->_attitude_target);

    // target angle velocity vector in the body frame
    Vector3f_t ang_vel_body_feedforward;
    quat_vec_rotate(&rotation_target_to_body,&ang_vel_body_feedforward, &att_ctrl->_ang_vel_target);

    // Correct the thrust vector and smoothly add feedforward and yaw input
    att_ctrl->_feedforward_scalar = 1.0f;
    if (att_ctrl->_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) {
        att_ctrl->_ang_vel_body.z = att_ctrl->_att_data.gyro.z;
    } else if (att_ctrl->_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) {
        att_ctrl->_feedforward_scalar = (1.0f - (att_ctrl->_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE);
        att_ctrl->_ang_vel_body.x += ang_vel_body_feedforward.x * att_ctrl->_feedforward_scalar;
        att_ctrl->_ang_vel_body.y += ang_vel_body_feedforward.y * att_ctrl->_feedforward_scalar;
        att_ctrl->_ang_vel_body.z += ang_vel_body_feedforward.z;
        att_ctrl->_ang_vel_body.z = att_ctrl->_att_data.gyro.z * (1.0 - att_ctrl->_feedforward_scalar) + att_ctrl->_ang_vel_body.z * att_ctrl->_feedforward_scalar;
    } else {
        vec3_add(&att_ctrl->_ang_vel_body, &att_ctrl->_ang_vel_body, &ang_vel_body_feedforward);
    }

    if (att_ctrl->_rate_bf_ff_enabled) {
        // rotate target and normalize
        Vector3f_t attitude_target_axis_angle = {att_ctrl->_ang_vel_target.x * att_ctrl->_dt, att_ctrl->_ang_vel_target.y * att_ctrl->_dt, att_ctrl->_ang_vel_target.z * att_ctrl->_dt};
        Quat_t attitude_target_update;
        quat_from_axis_angle1(&attitude_target_update, &attitude_target_axis_angle);
        
        att_ctrl->_attitude_target = quat_mult(&att_ctrl->_attitude_target, &attitude_target_update);
        quat_norm(&att_ctrl->_attitude_target);
    }

    // ensure Quaternion stay normalised
    quat_norm(&att_ctrl->_attitude_target);

    // Record error to handle EKF resets
    att_ctrl->_attitude_ang_error = quat_mult(&attitude_body_inv, &att_ctrl->_attitude_target);
}

// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration.
void attctrl_thrust_heading_rotation_angles(Attitude_ctrl * att_ctrl, Quat_t* attitude_target, const Quat_t* attitude_body, Vector3f_t* attitude_error, float* thrust_angle, float* thrust_error_angle)
{
    Quat_t thrust_vector_correction;
    attctrl_thrust_vector_rotation_angles(attitude_target, attitude_body, &thrust_vector_correction, attitude_error, thrust_angle, thrust_error_angle);

    // Todo: Limit roll an pitch error based on output saturation and maximum error.

    // Limit Yaw Error based on maximum acceleration - Update to include output saturation and maximum error.
    // Currently the limit is based on the maximum acceleration using the linear part of the SQRT controller.
    // This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters.
    Quat_t yaw_vec_correction_quat;
    if (!math_flt_zero(att_ctrl->_p_angle_yaw._kp) && fabsf(attitude_error->z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / att_ctrl->_p_angle_yaw._kp) {
        attitude_error->z = math_constrain_float(math_wrap_PI(attitude_error->z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / att_ctrl->_p_angle_yaw._kp, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / att_ctrl->_p_angle_yaw._kp);
        Vector3f_t yaw_vec = {0.0f, 0.0f, attitude_error->z};
        quat_from_axis_angle1(&yaw_vec_correction_quat, &yaw_vec);
        *attitude_target = quat_mult2(attitude_body, &thrust_vector_correction, &yaw_vec_correction_quat);
    }
}

// thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void attctrl_thrust_vector_rotation_angles(const Quat_t* attitude_target, const Quat_t* attitude_body, Quat_t* thrust_vector_correction, Vector3f_t* attitude_error, float* thrust_angle, float* thrust_error_angle)
{
    // The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
    const Vector3f_t thrust_vector_up = {0.0f, 0.0f, -1.0f};

    // attitude_target and attitute_body are passive rotations from target / body frames to the NED frame
    
    // Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame
    Vector3f_t att_target_thrust_vec;
    quat_vec_rotate(attitude_target, &att_target_thrust_vec, &thrust_vector_up); // target thrust vector

    // Rotating [0,0,-1] by attitude_target expresses (gets a view of) the current thrust vector in the inertial frame
    Vector3f_t att_body_thrust_vec;
    quat_vec_rotate(attitude_body, &att_body_thrust_vec, &thrust_vector_up); // current thrust vector

    // the dot product is used to calculate the current lean angle for use of external functions
    *thrust_angle = acosf(math_constrain_float(vec3_dot(&thrust_vector_up, &att_body_thrust_vec),-1.0f,1.0f));

    // the cross product of the desired and target thrust vector defines the rotation vector
    Vector3f_t thrust_vec_cross;
    vec3_cross(&thrust_vec_cross, &att_body_thrust_vec, &att_target_thrust_vec);

    // the dot product is used to calculate the angle between the target and desired thrust vectors
    *thrust_error_angle = acosf(math_constrain_float(vec3_dot(&att_body_thrust_vec, &att_target_thrust_vec), -1.0f, 1.0f));

    // Normalize the thrust rotation vector
    float thrust_vector_length = vec3_length(&thrust_vec_cross);
    if (math_flt_zero(thrust_vector_length) || math_flt_zero(*thrust_error_angle)) {
        thrust_vec_cross = thrust_vector_up;
    } else {
        vec3_mult(&thrust_vec_cross, &thrust_vec_cross, 1.0f/thrust_vector_length);
    }

    // thrust_vector_correction is defined relative to the body frame but its axis `thrust_vec_cross` was computed in
    // the inertial frame. First rotate it by the inverse of attitude_body to express it back in the body frame
    quat_inv_vec_rotate(attitude_body, &thrust_vec_cross, &thrust_vec_cross);
    quat_from_axis_angle2(thrust_vector_correction, &thrust_vec_cross, *thrust_error_angle);

    // calculate the angle error in x and y.
    Vector3f_t rotation;
    quat_to_axis_angle(thrust_vector_correction, &rotation);
    attitude_error->x = rotation.x;
    attitude_error->y = rotation.y;

    Quat_t thrust_vector_correction_inv;
    quat_inverse(&thrust_vector_correction_inv, thrust_vector_correction);

    Quat_t attitude_body_inv;
    quat_inverse(&attitude_body_inv, attitude_body);

    // calculate the remaining rotation required after thrust vector is rotated transformed to the body frame
    // heading_vector_correction
    Quat_t heading_vec_correction_quat = quat_mult2(&thrust_vector_correction_inv, &attitude_body_inv, attitude_target);

    // calculate the angle error in z (x and y should be zero here).
    quat_to_axis_angle(&heading_vec_correction_quat, &rotation);
    attitude_error->z = rotation.z;
}

// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f_t attctrl_update_ang_vel_target_from_att_error(Attitude_ctrl * att_ctrl, const Vector3f_t* attitude_error_rot_vec_rad)
{
    Vector3f_t rate_target_ang_vel;
    // Compute the roll angular velocity demand from the roll angle error
    if (att_ctrl->_use_sqrt_controller && !math_flt_zero(attctrl_get_accel_roll_max_radss(att_ctrl))) {
        rate_target_ang_vel.x = p_ctrl_sqrt_controller(attitude_error_rot_vec_rad->x, att_ctrl->_p_angle_roll._kp, math_constrain_float(attctrl_get_accel_roll_max_radss(att_ctrl) / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), att_ctrl->_dt);
    } else {
        rate_target_ang_vel.x = att_ctrl->_p_angle_roll._kp * attitude_error_rot_vec_rad->x;
    }

    // Compute the pitch angular velocity demand from the pitch angle error
    if (att_ctrl->_use_sqrt_controller && !math_flt_zero(attctrl_get_accel_pitch_max_radss(att_ctrl))) {
        rate_target_ang_vel.y = p_ctrl_sqrt_controller(attitude_error_rot_vec_rad->y, att_ctrl->_p_angle_pitch._kp, math_constrain_float(attctrl_get_accel_pitch_max_radss(att_ctrl) / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), att_ctrl->_dt);
    } else {
        rate_target_ang_vel.y = att_ctrl->_p_angle_pitch._kp * attitude_error_rot_vec_rad->y;
    }

    // Compute the yaw angular velocity demand from the yaw angle error
    if (att_ctrl->_use_sqrt_controller && !math_flt_zero(attctrl_get_accel_yaw_max_radss(att_ctrl))) {
        rate_target_ang_vel.z = p_ctrl_sqrt_controller(attitude_error_rot_vec_rad->z, att_ctrl->_p_angle_yaw._kp, math_constrain_float(attctrl_get_accel_yaw_max_radss(att_ctrl) / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), att_ctrl->_dt);
    } else {
        rate_target_ang_vel.z = att_ctrl->_p_angle_yaw._kp * attitude_error_rot_vec_rad->z;
    }
    return rate_target_ang_vel;
}

// translates body frame acceleration limits to the euler axis
Vector3f_t attctrl_euler_accel_limit(const Vector3f_t* euler_rad, const Vector3f_t* euler_accel)
{
    float sin_phi = math_constrain_float(fabsf(sinf(euler_rad->x)), 0.1f, 1.0f);
    float cos_phi = math_constrain_float(fabsf(cosf(euler_rad->x)), 0.1f, 1.0f);
    float sin_theta = math_constrain_float(fabsf(sinf(euler_rad->y)), 0.1f, 1.0f);

    Vector3f_t rot_accel;
    if (math_flt_zero(euler_accel->x) || math_flt_zero(euler_accel->y) || math_flt_zero(euler_accel->z) || math_flt_negative(euler_accel->x) || math_flt_negative(euler_accel->y) || math_flt_negative(euler_accel->z)) {
        rot_accel.x = euler_accel->x;
        rot_accel.y = euler_accel->y;
        rot_accel.z = euler_accel->z;
    } else {
        rot_accel.x = euler_accel->x;
        rot_accel.y = MIN(euler_accel->y / cos_phi, euler_accel->z / sin_phi);
        rot_accel.z = MIN(MIN(euler_accel->x / sin_theta, euler_accel->y / sin_phi), euler_accel->z / cos_phi);
    }
    return rot_accel;
}

// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using _input_tc
float attctrl_input_shaping_angle2(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt)
{
    // Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss
    desired_ang_vel += p_ctrl_sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);
    if (math_flt_positive(max_ang_vel)) {
        desired_ang_vel = math_constrain_float(desired_ang_vel, -max_ang_vel, max_ang_vel);
    }

    // Acceleration is limited directly to smooth the beginning of the curve.
    return attctrl_input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt);
}

// limits the acceleration and deceleration of a velocity request
float attctrl_input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt)
{
    // Acceleration is limited directly to smooth the beginning of the curve.
    if (math_flt_positive(accel_max)) {
        float delta_ang_vel = accel_max * dt;
        return math_constrain_float(desired_ang_vel, target_ang_vel - delta_ang_vel, target_ang_vel + delta_ang_vel);
    } else {
        return desired_ang_vel;
    }
}

// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void attctrl_euler_rate_to_ang_vel(const Vector3f_t* euler_rad, const Vector3f_t* euler_rate_rads, Vector3f_t* ang_vel_rads)
{
    float sin_theta = sinf(euler_rad->y);
    float cos_theta = cosf(euler_rad->y);
    float sin_phi = sinf(euler_rad->x);
    float cos_phi = cosf(euler_rad->x);

    ang_vel_rads->x = euler_rate_rads->x - sin_theta * euler_rate_rads->z;
    ang_vel_rads->y = cos_phi * euler_rate_rads->y + sin_phi * cos_theta * euler_rate_rads->z;
    ang_vel_rads->z = -sin_phi * euler_rate_rads->y + cos_theta * cos_phi * euler_rate_rads->z;
}

// limits angular velocity
void attctrl_ang_vel_limit(Vector3f_t* euler_rad, float ang_vel_roll_max, float ang_vel_pitch_max, float ang_vel_yaw_max)
{
    if (math_flt_zero(ang_vel_roll_max) || math_flt_zero(ang_vel_pitch_max)) {
        if (!math_flt_zero(ang_vel_roll_max)) {
            euler_rad->x = math_constrain_float(euler_rad->x, -ang_vel_roll_max, ang_vel_roll_max);
        }
        if (!math_flt_zero(ang_vel_pitch_max)) {
            euler_rad->y = math_constrain_float(euler_rad->y, -ang_vel_pitch_max, ang_vel_pitch_max);
        }
    } else {
        Vector2f_t thrust_vector_ang_vel = {euler_rad->x / ang_vel_roll_max, euler_rad->y / ang_vel_pitch_max};
        float thrust_vector_length = vec2_length(&thrust_vector_ang_vel);
        if (thrust_vector_length > 1.0f) {
            euler_rad->x = thrust_vector_ang_vel.x * ang_vel_roll_max / thrust_vector_length;
            euler_rad->y = thrust_vector_ang_vel.y * ang_vel_pitch_max / thrust_vector_length;
        }
    }
    if (!math_flt_zero(ang_vel_yaw_max)) {
        euler_rad->z = math_constrain_float(euler_rad->z, -ang_vel_yaw_max, ang_vel_yaw_max);
    }
}

// Convert an angular velocity vector to a 321-intrinsic euler angle derivative
// Returns false if the vehicle is pitched 90 degrees up or down
bool attctrl_ang_vel_to_euler_rate(const Vector3f_t* euler_rad, const Vector3f_t* ang_vel_rads, Vector3f_t* euler_rate_rads)
{
    float sin_theta = sinf(euler_rad->y);
    float cos_theta = cosf(euler_rad->y);
    float sin_phi = sinf(euler_rad->x);
    float cos_phi = cosf(euler_rad->x);

    // When the vehicle pitches all the way up or all the way down, the euler angles become discontinuous. In this case, we just return false.
    if (math_flt_zero(cos_theta)) {
        return false;
    }

    euler_rate_rads->x = ang_vel_rads->x + sin_phi * (sin_theta / cos_theta) * ang_vel_rads->y + cos_phi * (sin_theta / cos_theta) * ang_vel_rads->z;
    euler_rate_rads->y = cos_phi * ang_vel_rads->y - sin_phi * ang_vel_rads->z;
    euler_rate_rads->z = (sin_phi / cos_theta) * ang_vel_rads->y + (cos_phi / cos_theta) * ang_vel_rads->z;
    return true;
}

// Sets yaw target to vehicle heading and sets yaw rate to zero
// If reset_rate is false rates are not reset to allow the rate controllers to run
void attctrl_reset_yaw_target_and_rate(Attitude_ctrl * att_ctrl, bool reset_rate)
{
    // move attitude target to current heading
    float yaw_shift = quat_get_yaw(&att_ctrl->_att_data.attitude_body) - att_ctrl->_euler_angle_target.z;

    Quat_t _attitude_target_update;
    Vector3f_t yaw_shift_axis = {0.0f, 0.0f, yaw_shift};
    
    quat_from_axis_angle1(&_attitude_target_update, &yaw_shift_axis);
    att_ctrl->_attitude_target = quat_mult(&_attitude_target_update, &att_ctrl->_attitude_target);

    if (reset_rate) {
        // set yaw rate to zero
        att_ctrl->_euler_rate_target.z = 0.0f;

        // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
        attctrl_euler_rate_to_ang_vel(&att_ctrl->_euler_angle_target, &att_ctrl->_euler_rate_target, &att_ctrl->_ang_vel_target);
    }
}

/*------------------------------------test------------------------------------*/


